#!/usr/bin/env python
# -*- coding: utf-8 -*-

import matplotlib.pyplot as plt
import numpy as np
import math

#q_array=[[0,50,150,100,40],[0,90,150,180,0],[0,20,80,100,150],[180,100,150,50,100]]#指定起止位置
q_array=[[0,90,150,180,0],[0,20,80,100,150]]#指定起止位置
t_array=[0,3,6,12,14]#指定起止时间
v_array=[0,10,20,-15,0]#指定起止速度
a_array=[0,20,30,-20,0]#指定起止加速度
#初始状态
t=[t_array[0]]
q=[q_array[0][0]]
v=[v_array[0]]
a=[a_array[0]]

for i in range(len(q_array)):
    for j in range(len(q_array[i])-1):
        T = t_array[j + 1] - t_array[j]  # 时间差
        a0 = q_array[i][j]
        a1 = v_array[j]
        a2 = a_array[j] / 2
        a3 = (20 * q_array[i][j + 1] - 20 * q_array[i][j] - (8 * v_array[j + 1] + 12 * v_array[j]) * T - (3 * a_array[j] - a_array[j + 1]) * math.pow(T, 2)) / (2 * math.pow(T, 3))
        a4 = (30 * q_array[i][j] - 30 * q_array[i][j + 1] + (14 * v_array[j + 1] + 16 * v_array[j]) * T + (3 * a_array[j] - 2 * a_array[j + 1]) * math.pow(T, 2)) / (2 * math.pow(T, 4))
        a5 = (12 * q_array[i][j + 1] - 12 * q_array[i][j] - (6 * v_array[j + 1] + 6 * v_array[j]) * T - ( a_array[j] - a_array[j + 1]) * math.pow(T, 2)) / (2 * math.pow(T, 5))
        ti = np.arange(t_array[j], t_array[j + 1], .001)

        '''
        求解出角度，角速度，角加速度随某个时间区间随时间变换的曲线
        '''

        qi = a0 + a1 * (ti - t_array[j]) + a2 * np.power((ti - t_array[j]), 2) + a3 * np.power((ti - t_array[j]), 3) + a4 * np.power( (ti - t_array[j]), 4) + a5 * np.power((ti - t_array[j]), 5)
        vi = a1 + 2 * a2 * (ti - t_array[j]) + 3 * a3 * np.power((ti - t_array[j]), 2) + 4 * a4 * np.power( (ti - t_array[j]), 3) + 5 * a5 * np.power((ti - t_array[j]), 4)
        ai = 2 * a2 + 6 * a3 * (ti - t_array[j]) + 12 * a4 * np.power((ti - t_array[j]), 2) + 20 * a5 * np.power((ti - t_array[j]), 3)
        ti = ti.tolist()  # 将矩阵转换为list，否则进行数据整合会报错
        qi = qi.tolist()  # 将矩阵转换为list，否则进行数据整合会报错
        vi = vi.tolist()  # 将矩阵转换为list，否则进行数据整合会报错
        ai = ai.tolist()  # 将矩阵转换为List，否则进行数据整合会报错
        t = t + ti[1:] #进行数据整合，用来绘制函数图像
        q = q + qi[1:] #进行数据整合，用来绘制函数图像
        v = v + vi[1:] #进行数据整合，用来绘制函数图像
        a = a + ai[1:] #进行数据整合，用来绘制函数图像
    plt.figure(i)
    plt.plot(t, q, linestyle=":", color=(1, 0, 0, 1.0), label="angle curve")
    plt.scatter(t_array, q_array[i], marker='*', s=70, label="angle spot", zorder=3)

    plt.plot(t, v, linestyle="-", color=(1, 0.5, 1, 1.0), label="speed curve")
    plt.scatter(t_array, v_array, marker='^', s=70, label="speed spot", zorder=3)

    plt.plot(t, a, linestyle="-.", color=(0, 0, 0, 1.0), label="acceleration curve")
    plt.scatter(t_array, a_array, marker='o', s=70, label="acceleration spot", zorder=3)

    plt.grid()#显示网格
    plt.legend()#显示图例
    #保存图片
    figpath = 'E:/Python代码/机械臂运动轨迹规划/servo'+i.__str__()
    plt.savefig(figpath)
    t.clear()
    q.clear()
    v.clear()
    a.clear()
plt.show()
